#include "robot_interface.h"
#include "robot_control/Motion.h"
#include "geometry_msgs/Twist.h"
#include "parameter_server.h"
#include "planar_mapping.h"
#include <pcl_ros/transforms.h>

#define TW_MIN_ROTATION_SPEED 15
#define TW_MIN_TRASL_SPEED  20
#define ANGLE_TOLERANCE 0.05

RobotInterface::RobotInterface(PlanarMapping *pm) :
	move_client_("move_base", true) {
		mapper_ = pm;
		robot_type_ = ParameterServer::instance()->get<int>("robot_type");

		switch (robot_type_) {
			case TWENTE: {
					     motion_pub_ = nh_.advertise<robot_control::Motion>(
							     "motion_control", 10);
					     odometry_sub_ = nh_.subscribe<robot_control::Trajectory>(
							     "robot_trajectory", 1000,
							     &RobotInterface::TwenteCallback, this);
					     ROS_INFO("Robot type is TWENTE");
					     break;
				     }
			case P3DX: {
					   //motion_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
					   odometry_sub_ = nh_.subscribe<nav_msgs::Odometry>(
							   "/pose", 1000, &RobotInterface::P3dxCallback, this);
					   ROS_INFO("Robot type is P3DX");
					   break;
				   }
			default:
				   ROS_FATAL("Unknown robot type %d", robot_type_);
		}
	}

void RobotInterface::P3dxCallback(const nav_msgs::OdometryConstPtr& msg) {
	double velocity = msg->twist.twist.linear.x;
	double rotation_speed = msg->twist.twist.angular.z;
	int priority = DEFAULT_PRIORITY;

	double x = msg->pose.pose.position.x; //Maybe we need to scale
	double y = msg->pose.pose.position.y;
	double rot = tf::getYaw(msg->pose.pose.orientation); //Maybe we need to rotate

	//ROS_INFO("Received odometry %lf %lf %lf", x, y, rot);
	mapper_->odometryCallback(x, y, rot, velocity, rotation_speed, priority);
}

void RobotInterface::TwenteCallback(const robot_control::TrajectoryConstPtr& msg) {
	mapper_->odometryCallback(msg->x, msg->y, msg->rot,	msg->velocity,
			msg->rotation_speed, msg->priority);
}

bool goal_active = false;
void doneCb(const actionlib::SimpleClientGoalState& state,
 const move_base_msgs::MoveBaseResultConstPtr& result)
{
	goal_active = false;
	ROS_INFO("Goal reached");
}

// Called once when the goal becomes active
void activeCb()
{
}

// Called every time feedback is received for the goal
void feedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& result)
{
}

void RobotInterface::setMotion(double linear, double rotation) const {
	if (!robot_type_) {
		ROS_FATAL("Unknown robot type");
		return;
	}

	bool active_control = false;
	nh_.param("/rgbdslam/active_control", active_control, false);
	if (!active_control)
		return;

	//ROS_INFO("Motion command -> linear: %d rotation: %d", linear, rotation);
	switch (robot_type_) {
		case TWENTE: {
				     int linear_speed = 0;
				     int rotation_speed = 0;

				     if (rotation >= ANGLE_TOLERANCE) {
					     rotation_speed = TW_MIN_ROTATION_SPEED;
				     } 
				     else if(rotation <= -ANGLE_TOLERANCE){
					     rotation_speed = -TW_MIN_ROTATION_SPEED;
				     }
				     else {
					     linear_speed = TW_MIN_TRASL_SPEED;
				     }

				     robot_control::Motion motion_msg;
				     motion_msg.priority = DEFAULT_PRIORITY;
				     motion_msg.speed = linear_speed;
				     motion_msg.rotation = rotation_speed;
				     motion_msg.computing_path = 1000;
				     motion_pub_.publish(motion_msg);
				     break;
			     }

		case P3DX: {
				   if (goal_active)
				   	return;
				   goal_active = true;
				   move_base_msgs::MoveBaseGoal goal;
				   goal.target_pose.header.frame_id = "base_link";
				   goal.target_pose.header.stamp = ros::Time::now();

				   if (linear > 0)
				   	goal.target_pose.pose.position.x = (float) 0.2;

				   if (abs(rotation) < ANGLE_TOLERANCE)
                                     rotation = 0;

				   btQuaternion q;
				   q.setEulerZYX(-rotation, 0, 0);
				   goal.target_pose.pose.orientation.x = q.x();
				   goal.target_pose.pose.orientation.y = q.y();
				   goal.target_pose.pose.orientation.z = q.z();
				   goal.target_pose.pose.orientation.w = q.w();

				   ROS_INFO("Motion cmd %lf %lf", goal.target_pose.pose.position.x,
					-rotation);

				   move_client_.cancelAllGoals();
				   move_client_.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
				   break;
			   }
	}
}
